#include "GLFWApp.h"
#include "Shapes3D.h"

#include "fonts/bitmap_font.h"
#include <cstdlib>
#include <map>
#include <string>
#include <list>

#include <utils.hpp>
using siut::simd::Vec3f;
using siut::simd::Mat4f;

#include "verlet_engine.h"

class TrailMaker
{
public:
	std::vector< Vec2f > trails;
	SpringBody *trackedBody;

	TrailMaker(real x, real y, SpringBody *body)
	{
		trackedBody = body;
		body->calcCenter();
		for(int i=0; i<40; i++)
		trails.push_back( body->centroid );
	}

	~TrailMaker()
	{
		trackedBody = 0x0;
	}

	void update()
	{
		
		for(size_t i=1; i<trails.size(); i++)
		{
			trails[i-1] = trails[i];
		}
		trails[trails.size()-1] = trackedBody->centroid;
	}

	void draw()
	{
		glColor3f(1,1,1);
		glBegin(GL_LINE_STRIP);
		for(size_t i=0; i<trails.size(); i++)
		{
		glVertex2f( trails[i].x() ,trails[i].y() );
		}
		glEnd();
	}
};

class MyApp : public GLFWApp
{
public:
	MyApp() : GLFWApp()
	{
		init();
		mainloop();
	}

	virtual ~MyApp()
	{
		printf("dtor myapp %s\n",__FILE__);
	}


	void init()
	{
		zoom = 15;

		glfwSwapInterval(1);
		//glfwDisable(GLFW_MOUSE_CURSOR);
		BitmapFont::Init();

		player = SpringFactory::makeBox( &physWorld, -2.5,-2.5,0.5,0.5 );

		playerThrustGenerator = new ThrustGenerator();
		physWorld.registerForceGenerator(playerThrustGenerator, player);

		for(int i=0; i<10; i++)
		{
			real xpos = i * 11.5f;
			real ypos = real(rand()) / RAND_MAX - 0.5;
			ypos *= 12;
			
			SpringBody *planet = SpringFactory::makeSphere( &physWorld, xpos,ypos, 2.0f, true);
			planet->setFrozen();

			IForceGenerator *gforce = new GravityForce(xpos, ypos);
			gravityGenerators.push_back( gforce );

			physWorld.registerForceGenerator( gforce, player );
		}


	}

	// \breif Expects OpenGL modelview matrix to be set to world view. for instance with gluLookAt
	void updateWorldMouseCoordinates()
	{
		float ratioX = 16 * zoom * 1.0 / 15.0;
		float ratioY = 12 * zoom * 1.0 / 15.0;

		Vec2f mousePos( Input::mouseX()/(xres/1.0f) * ratioX - ratioX/2.0f, -Input::mouseY()/(yres/1.0f) * ratioY + ratioY/2.0f );

		float mouseSpeed = 1.0;
		if( Input::MouseHit(GLFW_MOUSE_BUTTON_MIDDLE) )
		{
			glfwSetMousePos(xres/2,yres/2);
		}
		//Vec2f mousePos( Input::mouseX()/(xres/1.0f) * mouseSpeed*zoom*ratioX - zoom*ratioX/2.0f, -Input::mouseY()/(yres/1.0f) * mouseSpeed*zoom*ratioY - zoom*ratioY/2.0f );

		Vec3f centerOfInterest( (float)player->centroid.x(), (float)player->centroid.y(), 0.0f );

		float mtx[16];
		glGetFloatv( GL_MODELVIEW_MATRIX, mtx );

		Mat4f mvp( mtx );
		Mat4f mvp_Inv;
		mvp_Inv = inverse(mvp);


		mousePointer = siut::simd::mousePosOnInterestPlaneAsObjectCoords(mvp, mvp_Inv, centerOfInterest, mousePos);
	}


	void update()
	{
		physWorld.startFrame();

		double physTimeSpent = glfwGetTime();
		physWorld.update( dt );
		physTimeSpent = glfwGetTime() - physTimeSpent;

		real vx = Input::keyDown(GLFW_KEY_RIGHT) - Input::keyDown(GLFW_KEY_LEFT);
		real vy = Input::keyDown(GLFW_KEY_UP) - Input::keyDown(GLFW_KEY_DOWN);

		vx = Input::keyDown('D') - Input::keyDown('A');
		vy = Input::keyDown('W') - Input::keyDown('S');

		if( Input::keyHit('P') )
		{
			for each(Particle *p in player->particles )
			{
				p->oldPos = p->pos;
			}	
		}

		if( player->isColliding )
		if( Input::keyDown(GLFW_KEY_SPACE) )
		{
			// get closest gravity fields normal
			int closest = -1;
			real minDist = 1e32;
			for(size_t i=0; i<gravityGenerators.size(); i++)
			{
				GravityForce* gforceGenerator = dynamic_cast<GravityForce*>( gravityGenerators[i] );

				if (gforceGenerator != 0)
				{
					real distSq = length_2( player->centroid - gforceGenerator->gravitySourcePos );
					if( distSq < minDist )
					{
						minDist = distSq;
						closest = i;
					}
				}
			}
			if( closest != -1)
			{
				GravityForce* gforceGenerator = dynamic_cast<GravityForce*>( gravityGenerators[closest] );
				Vec2f normalTo = normalize( player->centroid - gforceGenerator->gravitySourcePos );
				//player->addForce( 1 * normalTo );
				playerThrustGenerator->timeLeft = 1.0;
				playerThrustGenerator->thrustDir = normalTo / 100.0;
			}

			
		}

		player->addForce( 0.01 * Vec2f(vx,vy) );

		bool spawnBox = false;
		real boxSiz = 0;
		if( Input::MouseHit(GLFW_MOUSE_BUTTON_LEFT) )
		{
			spawnBox = true;
			boxSiz = 0.5;
		}
		if( Input::MouseHit(GLFW_MOUSE_BUTTON_RIGHT) )
		{
			spawnBox = true;
			boxSiz = 0.25;
		}

		if( spawnBox )
		{
			real spawnX = mousePointer.x()-0.25;
			real spawnY = mousePointer.y()-0.25;
			
			SpringBody *box = SpringFactory::makeBox(&physWorld,spawnX, spawnY,boxSiz,boxSiz);
			TrailMaker *tmaker = new TrailMaker(spawnX, spawnY, box);

			for each(Particle *p in box->particles)
			{
				p->massInverse = 1.0;
			}

			for each( IForceGenerator *fgen in gravityGenerators )
			{
				physWorld.registerForceGenerator( fgen, box );
			}

			trailMakerList.push_back( tmaker );
			
		}

		if( Input::keyHit(GLFW_KEY_ESC) )
		{
			this->gameRunning = false;
		}

		if( numframes % 2 == 0)
		for(size_t i=0; i<trailMakerList.size(); i++){
			trailMakerList[i]->update();
		}

		int mz = glfwGetMouseWheel();
		
		zoom -= mz * 1.5;
		zoom = std::max<float>( 5.0f, zoom);
		zoom = std::min<float>( zoomMax, zoom);
		glfwSetMouseWheel(0);

		// todo fix code of fail:
		double desiredMSPF = 0.017;

		if( dt < desiredMSPF )
		{
			double timeSlept = std::max(0.0, desiredMSPF - dt);
			glfwSleep( timeSlept );
		}
	}
	
	void render()
	{
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		setDepthProjection();
		glEnable(GL_DEPTH_TEST);
		//glDepthFunc(GL_LEQUAL);
		glLoadIdentity();

		gluLookAt( player->centroid.x(), player->centroid.y(), zoom, 
						   player->centroid.x(), player->centroid.y(), 0.0,
							 0.0, 1.0, 0.0);

		updateWorldMouseCoordinates();

		glPushMatrix();
		glPushAttrib(GL_POLYGON_BIT);
		glTranslatef( mousePointer.x(), mousePointer.y(), mousePointer.z() );
		glColor3f(1,0,0);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		glRotatef( numframes, 0,1,1 );
		Shapes3D::DrawSphere(0.25, 5, 5);
		glPopAttrib();
		glPopMatrix();

		glColor3f(1,1,1);
		physWorld.draw();

		glDisable(GL_DEPTH_TEST);

		if( playerThrustGenerator->timeLeft > 0.0)
		{
			Vec2f thrustDir = playerThrustGenerator->thrustDir;
			glColor3f(1,1,0);
			glBegin(GL_LINES);
			glVertex2f( player->centroid.x(), player->centroid.y() );
			glVertex2f( player->centroid.x() + thrustDir.x()*150.0f, player->centroid.y() + thrustDir.y()*150.0f );
			glEnd();

			if( numframes % 5 == 0 )
			{
				Vec2f tDirNormalized = normalize(thrustDir);
				real spawnX = player->centroid.x() - tDirNormalized.x() * 0.4;
				real spawnY = player->centroid.y() - tDirNormalized.y() * 0.4;
				
				real boxSiz = 0.2;
				if( physWorld.isSpaceEmpty(spawnX+boxSiz, spawnY-boxSiz, spawnX-boxSiz, spawnY+boxSiz ) )
				{
					SpringBody *box = SpringFactory::makeTriangle(&physWorld,spawnX, spawnY,boxSiz);
					TrailMaker *tmaker = new TrailMaker(spawnX, spawnY, box);

					for each(Particle *p in box->particles)
					{
						p->massInverse = 1.0;
					}
					for each( IForceGenerator *fgen in gravityGenerators )
					{
						physWorld.registerForceGenerator( fgen, box );
					}

					trailMakerList.push_back( tmaker );
				}

				glBegin(GL_LINE_LOOP);
				glVertex2f(spawnX-boxSiz, spawnY-boxSiz); 
				glVertex2f(spawnX+boxSiz, spawnY-boxSiz ); 
				glVertex2f(spawnX+boxSiz, spawnY+boxSiz ); 
				glVertex2f(spawnX-boxSiz, spawnY+boxSiz ); 
				glEnd();
				

			}

		}


		for(size_t i=0; i<trailMakerList.size(); i++){
			trailMakerList[i]->draw();
		}
		real siz = 0.25;
		real minx = mousePointer.x() - siz;
		real maxx = mousePointer.x() + siz;
		real miny = mousePointer.y() - siz;
		real maxy = mousePointer.y() + siz;
		if( physWorld.isSpaceEmpty(minx,miny,maxx,maxy) )
		{
			glColor3f(0,1,0);
			glBegin(GL_QUADS);
		}else
		{
			glColor3f(1,1,0);
			glBegin(GL_LINE_LOOP);
		}
		
		glVertex2f(minx,miny); 
		glVertex2f(maxx,miny); 
		glVertex2f(maxx,maxy); 
		glVertex2f(minx,maxy); 
		glEnd();
		
		setOrthoProjection();
		glLoadIdentity();

		char charBuf[200];
		sprintf_s( charBuf, 200, "fps = %f", fps );
		BitmapFont::Print(charBuf, 15,15,0xff0000);
		sprintf_s( charBuf, 200, "dt = %f", dt );
		BitmapFont::Print(charBuf, 15,15*2,0x00ff00);
		sprintf_s( charBuf, 200, "num body objects: %i", physWorld.bodyList.size() );
		BitmapFont::Print(charBuf, 15,15*5, 0xff000);

		sprintf_s( charBuf, 200, "Press ESC to quit!" );
		BitmapFont::Print(charBuf, 15,15*8, 0xffffff);	
	}

private:
	World physWorld;
	SpringBody *player;
	Vec3f mousePointer;
	std::vector< IForceGenerator* > gravityGenerators;
	ThrustGenerator *playerThrustGenerator;
	std::vector< TrailMaker* > trailMakerList;
	

	float zoom;
	static const int zoomMax = 50;

};